#ifndef JOINT_TRACKING_CONTROLLER_H
#define JOINT_TRACKING_CONTROLLER_H

#include "Controller.h"
#include "Modeling/Ramp.h"
//#include "Continuous/MilestonePath.h"

/** @brief A controller base class that reads out a desired servo position
 * and velocity using the method GetDesiredState.
 */
class JointTrackingController : public RobotController
{
 public:
  JointTrackingController(Robot& robot);
  virtual ~JointTrackingController() {}  
  virtual void Update(Real dt);
  virtual void Reset();
  virtual bool ReadState(File& f) {
    if(!RobotController::ReadState(f)) return false;
    if(!qdesDefault.Read(f)) return false;
    return true;
  }
  virtual bool WriteState(File& f) const {
    if(!RobotController::WriteState(f)) return false;
    if(!qdesDefault.Write(f)) return false;
    return true;
  }

  ///subclasses should override this
  virtual void GetDesiredState(Config& q_des,Vector& dq_des);

  Config qdesDefault;
};

/** @brief A controller that can be commanded to reach configurations
 * (with or without velocities specified) smoothly using acceleration
 * bounded ramps.
 * 
 * A SetMilestone command constructs a smooth acceleration bounded curve
 * to reach the desired configuration (or configuration + velocity).
 *
 * A SetLinearVelocity sets a linear trajectory from the current
 * configuration x0 along the curve x(0) = x0+dx*t for t <= tmax.
 * Care must be taken to avoid discontinuous changes in velocity.
 */
class MilestoneTrackingController : public JointTrackingController
{
 public:
  MilestoneTrackingController(Robot& robot);
  virtual ~MilestoneTrackingController() {}
  void SetMilestone(const Vector& x,const Vector& dx);
  void SetMilestone(const Vector& x);
  void SetMilestone(const Vector& x,Real minTime);
  void SetLinearVelocity(const Vector& dx,Real tmax);

  virtual void GetDesiredState(Config& q_des,Vector& dq_des);
  virtual void Update(Real dt);
  virtual void Reset();
  virtual bool ReadState(File& f);
  virtual bool WriteState(File& f) const;
  bool Reached() const { return pathParameter >= ramp.endTime; }
  Real TimeRemaining() const { return Max(ramp.endTime-pathParameter,0.0); }
  Real GetSpeedScale(const Config& q_des,const Vector& dq_des) const;
  const Config& Destination() const { return ramp.x1; }
  const Vector& DestinationVelocity() const { return ramp.dx1; }

  /// Current commanded position and orientation
  Vector xcur,dxcur;
  /// A smooth ramp to reach the current destination.  xcur is
  /// the point on the ramp evaluated at pathParameter.  dxcur is the
  /// velocity of the ramp at pathParameter, scaled by the current speed
  /// modification (if any).
  ParabolicRampND ramp;
  Real pathParameter;

  // Settings
  /// A scaling of the robot's velocity and acceleration bounds
  /// Note: for loading and saving state, these should not be modified during execution!
  Real velScale,accScale;
  /// If modifySpeedByError is set to true, slows down the advancement of
  ///the path by 1/(1+modifySpeedCoeff*err) where err is the sum of
  ///squared errors between the actual and commanded configuration
  bool modifySpeedByError;
  Real modifySpeedCoeff;
};

#endif
